#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright    :   Jihua Lab 2021.
@File    :   network_state_pub.py
@Author  :   WANG Hao 
@Contact :   wanghao@jihualab.com、haowanghk@gmail.com(Github)
@License :   (C)Copyright 2020-2021, JHL
@Version :   1.1
@Create Time    :   CURRENT_YEAR/CURRENT_MONTH/CURRENT_DATE CURRENT_HOUR:CURRENT_MINUTE:CURRENT_SECOND
@Modify Time      @Author    @Version    @Desciption
-------------------        --- -------    ---  --------   --- -----------
@File Description :   Network check and ping host ip address response publisher
'''

# here put the import lib

# the import lib
import logging
import subprocess
import re

import time

# inport ros py
import rospy  
from dual_arm_base_control.msg import NetworkState

from std_msgs.msg import String

####### PING HOST IP THREADING #####
# ping_host_ip return HOST_STATE = False or True
def ping_host_ip(this_ip, host_ip, msg):
    # 记录开始执行的时间
    start_time = time.time()
    retry_times = 0
    count = 2
    interval = 0.5
    size = 2
    # init the state and avg ping results
    HOST_STATE = False
    msg.avgPingResponse = 0
    # 定义用来 ping host ip
    if (this_ip and host_ip) != None: 
        # 构造ping命令
        command = 'ping -c %s' % count + " -i %s" % interval + " -w %s" % size + " %s" % host_ip
        try:
            # <subprocess.Popen object at 0x7f6f93d6df10>
            res = subprocess.Popen([command],stdin=subprocess.PIPE,
                                                stdout=subprocess.PIPE,
                                                stderr = subprocess.PIPE,shell = True) 

        except subprocess.CalledProcessError,err:
            rospy.loginfo("HOST_IP_CHECK: WARNING - The HOST IP {} is NOT reachable! retry_times {} !".format(host_ip,
                                                                        retry_times))
        if res != None:
            rospy.loginfo("HOST_IP_CHECK: The HOST_IP {} is reachable! STATE INFO has updated! ".format(host_ip))
            rospy.loginfo('HOST_IP_CHECK: Time used ：%s' % (time.time() - start_time))
            
            # 提取返回值
            out = res.stdout.read().decode('utf-8')
            # printing ping results
            rospy.loginfo("---Host ip Ping results PRINTING: {}".format(out))
            regex = r'time=(.+?)ms'
            ping_results = re.findall(regex, out)

            # ping 延迟结果排序
            ping_results.sort()
            #检查下标
            index = int(0.1*int(count))
            if len(ping_results) == 0:
                print("HOST_IP_CHECK: Error!ping_results No data!")
                HOST_STATE = False  # ping连通性，结束循环
    
            elif len(ping_results) < index :
                print("HOST_IP_CHECK: Error! Index out of range!")
                HOST_STATE = False  # ping连通性，结束循环
            else:
                    #change the host ip state into True
                msg.avgPingResponse = ping_results[index].encode('utf8')
                print(ping_results[index])
                HOST_STATE = True  # ping连通性，True
                
        else:   
            rospy.loginfo("HOST_IP_CHECK: WARNING - The HOST IP {} is NOT reachable! ".format(host_ip))
            rospy.loginfo("HOST_IP_CHECK: WARNING - HOST IP {} PING retry {} Times! ".format(host_ip, retry_times))
            retry_times = retry_times + 1
    else:
        rospy.logerr("HOST_IP_CHECK: ERROR - error form This or Host ip {} / {} ,PLEASE check Parameter Server IP INFO !".format(this_ip,host_ip))
        HOST_STATE = False  # the input host ip address error, check the host ip

    # assignment the network state 
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = 'network_monitor_frame'
    msg.state = HOST_STATE

    return msg          #   disconnected - change the host ip state flag into False


def main():
    # init the ropy nodelet and paramters defianation
    rospy.init_node(rospy.get_param('/node_info/network_pub_node_name'), 
                                    anonymous=True)
    is_debug = rospy.get_param('/general/is_debug')


    # get the host and this machineip address
    this_ip = rospy.get_param('/general/this_ip')
    host_ip = rospy.get_param('/general/host_ip')   

    # define the publisher
    try:
        network_pub = rospy.Publisher(rospy.get_param('/node_info/network_pub_topic_name'),
                    NetworkState,
                    queue_size = rospy.get_param('/node_info/network_pub_queue_size'))

        rospy.loginfo("-----Network State Publsiher Topic init success!-----")
    except rospy.ROSException:
        rospy.logerr("-----Network State Publsiher Topic init ERROR !-----")
        return -1
    
    pub_freq = rospy.get_param('/node_info/network_pub_freq')
    pub_rate = rospy.Rate(pub_freq)         # default pub frequence is 0.1 
    #### Create Network State Info ####
    msg = NetworkState()
    while not rospy.is_shutdown():
        msg_pub  = ping_host_ip(this_ip,host_ip,msg)
        network_pub.publish(msg_pub)
        pub_rate.sleep()

# main function
if __name__ == '__main__':
    # 创建一个logger_debug
    import sys

    #logger = logging.getLogger()
    #logger.setLevel(logging.DEBUG)  # Log等级总开关-debug
    #print("Logging Set Info: logger.setLevel(logging.DEBUG") 
    try:
        main()    
    except KeyboardInterrupt:
        rospy.loginfo("-----KeyboardInterrupt: 主程序正在退出!-----")

